/*
* Copyright (c) 2007 by Kirill Kolodyazhniy.
* See the file "license.terms" for information on usage and redistribution.
*/

#include "odeutil.h"

namespace Engine
{	
	/***************************************************************************************************************/
	void OdeUtil::Convert(const Matrix4<REAL>& transform, dMatrix3* rot, dReal* x, dReal* y, dReal* z) 
	{
		(*rot)[0] = static_cast<dReal>(transform[0][0]);
		(*rot)[4] = static_cast<dReal>(transform[1][0]);
		(*rot)[8] = static_cast<dReal>(transform[2][0]);
		(*rot)[3] = 0;
		

		(*rot)[1] = static_cast<dReal>(transform[0][1]);
		(*rot)[5] = static_cast<dReal>(transform[1][1]);
		(*rot)[9] = static_cast<dReal>(transform[2][1]);
		(*rot)[7] = 0;
		

		(*rot)[2]  = static_cast<dReal>(transform[0][2]);
		(*rot)[6]  = static_cast<dReal>(transform[1][2]);
		(*rot)[10] = static_cast<dReal>(transform[2][2]);
		(*rot)[11] = 0;

		*x = static_cast<dReal>(transform[0][3]);
		*y = static_cast<dReal>(transform[1][3]);
		*z = static_cast<dReal>(transform[2][3]);		
	}
	/***************************************************************************************************************/
	void OdeUtil::Convert(const dReal* rot, const dReal* pos, Matrix4<REAL>* matr)
	{
		(*matr)[0][0] = static_cast<REAL>(rot[0]);
		(*matr)[1][0] = static_cast<REAL>(rot[4]);
		(*matr)[2][0] = static_cast<REAL>(rot[8]);
		(*matr)[3][0] = 0;

		(*matr)[0][1] = static_cast<REAL>(rot[1]);
		(*matr)[1][1] = static_cast<REAL>(rot[5]);
		(*matr)[2][1] = static_cast<REAL>(rot[9]);
		(*matr)[3][1] = 0;

		(*matr)[0][2] = static_cast<REAL>(rot[2]);
		(*matr)[1][2] = static_cast<REAL>(rot[6]);
		(*matr)[2][2] = static_cast<REAL>(rot[10]);
		(*matr)[3][2] = 0;

		(*matr)[0][3] = static_cast<REAL>(pos[0]);
		(*matr)[1][3] = static_cast<REAL>(pos[1]);
		(*matr)[2][3] = static_cast<REAL>(pos[2]);
		(*matr)[3][3] = 1;
	}
	/***************************************************************************************************************/
}
